#include "v4l2.h"

// 1.打开UVC设备
CamId v4l2_init(const char* device) {
    CamId fd = open(device, O_RDWR);
    return fd;
}

// 2.获取UVC支持格式
Status v4l2_get_support_formats(CamId fd) {
    struct v4l2_fmtdesc v4fmt;
    printf("--- Print frame formats:\n");
    for (int i = 0; true; ++i) {
        memset(&v4fmt, 0, sizeof(v4fmt));
        v4fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        v4fmt.index = i;
        int ret = ioctl(fd, VIDIOC_ENUM_FMT, &v4fmt);
        if (ret < 0 && i == 0) 
            return ERROR_VIDIOC_ENUM_FMT;
        if (ret < 0) 
            break;
        printf(" |- index:%d\n", v4fmt.index);
        printf(" |- flags=%d\n", v4fmt.flags);
        printf(" |- description=%s\n", v4fmt.description);
        unsigned char *p = (unsigned char*)&v4fmt.pixelformat;
        printf(" |- pixelformat=%c%c%c%c\n", p[0], p[1], p[2], p[3]);
        printf(" |- reserved=%d\n |\n", v4fmt.reserved[0]);
    }
    return SUCCESS;
}

// 3.配置UVC采集格式
Status v4l2_set_capture_format(CamId fd, uint width, uint height) {
    struct v4l2_format vfmt;
    memset(&vfmt, 0, sizeof(vfmt));
    vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    vfmt.fmt.pix.width = width;
    vfmt.fmt.pix.height = height;
    //vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV; // YUYV 4:2:2
    vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG; // MJPEG
    int ret = ioctl(fd, VIDIOC_S_FMT, &vfmt);
    if (ret < 0) 
        return ERROR_VIDIOC_S_FMT;
    
    // 检查
    memset(&vfmt, 0, sizeof(vfmt));
    vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    ret = ioctl(fd, VIDIOC_G_FMT, &vfmt);
    if (ret < 0) 
        return ERROR_VIDIOC_G_FMT;
    if (vfmt.fmt.pix.width != width ||
        vfmt.fmt.pix.height != height ||
        vfmt.fmt.pix.pixelformat != V4L2_PIX_FMT_MJPEG
    )   return ERROR_VIDIOC_S_FMT;
    return SUCCESS;
}

// 4.申请内核缓冲区队列
Status v4l2_request_core_buffer(CamId fd) {
    struct v4l2_requestbuffers reqbuffer;
    memset(&reqbuffer, 0, sizeof(reqbuffer));
    reqbuffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    reqbuffer.count = 4;
    reqbuffer.memory = V4L2_MEMORY_MMAP;
    int ret = ioctl(fd, VIDIOC_REQBUFS, &reqbuffer);
    if (ret < 0)
        return ERROR_VIDIOC_REQBUFS;
    return SUCCESS;
}

// 5.把内核缓冲区队列映射到用户空间
Status v4l2_map_buffer(CamId fd, ImgBuf* &buffer) {
    struct v4l2_buffer mapbuffer;
    for (int i = 0; i < 4; ++i) {
        memset(&mapbuffer, 0, sizeof(mapbuffer));
        mapbuffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        mapbuffer.index = i;
        int ret = ioctl(fd, VIDIOC_QUERYBUF, &mapbuffer);
        if (ret < 0)
            return ERROR_VIDIOC_QUERYBUF;
        buffer[i].start = (uchar*)mmap(NULL, mapbuffer.length, PROT_READ|PROT_WRITE, 
                                            MAP_SHARED, fd, mapbuffer.m.offset);
        buffer[i].length = mapbuffer.length;
        ret = ioctl(fd, VIDIOC_QBUF, &mapbuffer);
        if (ret < 0)
            return ERROR_VIDIOC_QBUF;
    }
    return SUCCESS;
}

// 6.开始采集
Status v4l2_start_stream(CamId fd) {
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    int ret = ioctl(fd, VIDIOC_STREAMON, &type);
    if (ret < 0)
        return ERROR_VIDIOC_STREAMON;
    return SUCCESS;
}

// 7.采集数据 - 上锁
Status v4l2_get_frame(CamId fd, v4l2_buffer &readbuffer) {
    memset(&readbuffer, 0, sizeof(readbuffer));
    readbuffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    int ret = ioctl(fd, VIDIOC_DQBUF, &readbuffer);
    if (ret < 0) 
        return ERROR_VIDIOC_DQBUF;
    return SUCCESS;
}

// 8.采集数据 - 解锁
Status v4l2_release_frame(CamId fd, v4l2_buffer &readbuffer) {
    int ret = ioctl(fd, VIDIOC_QBUF, &readbuffer);
    if (ret < 0)
        return ERROR_VIDIOC_DQBUF;
    return SUCCESS;
}

// 9.停止采集
Status v4l2_stop_stream(CamId fd) {
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    int ret = ioctl(fd, VIDIOC_STREAMOFF, &type);
    if (ret < 0)
        return ERROR_VIDIOC_STREAMOFF;
    return SUCCESS;
}

// 10.释放映射
Status v4l2_munmap_buffer(ImgBuf* &buffer) {
    for (int i = 0; i < 4; ++i) 
        munmap(buffer[i].start, buffer[i].length);
    return SUCCESS;
}

// 11.关闭设备
Status v4l2_close(CamId fd) {
    if (close(fd) < 0) return ERROR_CLOSE_CAPTURE;
    return SUCCESS;
}